import smbus2 as smbus
from CacheDef import *


class MPU6050:
    def __init__(self, i2c_bus=5, i2c_address=0x68):
        self._i2c_bus = i2c_bus
        self.i2c_address = i2c_address
        self._i2c = smbus.SMBus()
        self._dev = None

    def open(self):
        self._i2c.open(bus=self._i2c_bus)


    def close(self):
        self._i2c.close()
        self._dev = None

    def _i2c_read(self, reg, data_p, length):
        ret_val = 0
        result = []

        try:
            result = self._i2c.read_i2c_block_data(self.i2c_address, reg, length)
        except IOError:
            ret_val = -1

        if ret_val == 0:
            for index in range(length):
                data_p[index] = result[index]

        return ret_val

    def _i2c_write(self, reg, data_p, length):
        ret_val = 0
        data = []

        for index in range(length):
            data.append(data_p[index])
        try:
            self._i2c.write_i2c_block_data(self.i2c_address, reg, data)
        except IOError:
            ret_val = -1

        return ret_val

    def i2c_write_byte(self, reg, data):
        return self._i2c_write(reg, [data], 1)

    def i2c_read_byte(self, reg, data_p):
        return self._i2c_read(reg, data_p, 1)


class MPU6050Api:
    def __init__(self, mpu6050: MPU6050):
        self.mpu6050 = mpu6050

    def init_6050(self):
        # recovery from sleep
        self.mpu6050.i2c_write_byte(PWR_MGMT_1, 0x00)
        self.mpu6050.i2c_write_byte(SMPLRT_DIV, 0x07)
        self.mpu6050.i2c_write_byte(CONFIG, 0x06)
        self.mpu6050.i2c_write_byte(GYRO_CONFIG, 0x18)
        self.mpu6050.i2c_write_byte(ACCEL_CONFIG, 0x01)

    def read_data(self, reg_address):
        H, L = [-1], [-1]
        H_ret = self.mpu6050.i2c_read_byte(reg_address, H)
        L_ret = self.mpu6050.i2c_read_byte(reg_address + 1, L)
        assert (H_ret | L_ret) != -1
        return self._convert_to_c_unsigned_int((self._convert_to_c_char(H[0]) << 8) + self._convert_to_c_char(L[0]))

    def _convert_to_c_char(self, char_value):
        return (char_value + 128) % 256 - 128

    def _convert_to_c_unsigned_int(self, int_value):
        #return int_value % (2 ** 32)
        # with positive and negative, ori code is error
        return int_value
